#include  "../include/zeus_s2_odom/zeus_s2_odom.h"


DriverOdomPub::DriverOdomPub():
        start_flag_(0),
        last_twist_time_(0),
        rev_left_(0),
        rev_left_1_(0),
        rev_right_(0),
        rev_right_1_(0),
        last_time_(0){
        }

DriverOdomPub::~DriverOdomPub()
{
    boost::mutex::scoped_lock look(mutex_);

    printf("DirvePub deinit.\n");

}
void DriverOdomPub::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        current_twist_ = *msg.get();
        cmd_vel_flag = true;
        cmd_vel_mutex_.unlock();

    }
    catch(const std::exception& e)
    {
        cmd_vel_mutex_.unlock();
        std::cerr << e.what() << '\n';
    }
}

//接收驱动器速度
void DriverOdomPub::motor_feedback_callback( const zeus_s2_msgs::Feedback::ConstPtr& msg)
{
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        current_speed_ = *msg.get();
        rev_speed_flag = true;
        cmd_vel_mutex_.unlock();

    }
    catch(const std::exception& e)
    {
        cmd_vel_mutex_.unlock();
        std::cerr << e.what() << '\n';
    }


}


void DriverOdomPub::handle_speed_data(void)
{
    now_ = ros::Time::now();
    delta_time_ = (now_ - last_time_).toSec();
    if(start_flag_)
    {
        accumulation_y_ = 0.0;
        accumulation_x_ = 0.0;
        accumulation_th_ = 0.0;

		accumulation_left_ = 0.0;
    	accumulation_right_ = 0.0;
		accumulation_right_1_ = 0.0;
		accumulation_left_1_ = 0.0;

		delta_left_ = 0;
        delta_right_ = 0;
		delta_left_1_= 0;
        delta_right_1_= 0;
        delta_time_ = 0;
        last_time_ = now_;
        start_flag_ = false;
        ROS_INFO("Drive topic and Odom successfully connected  !");
        // ROS_INFO_STREAM("The Code Version is" << code_version_);

    }

   delta_left_ =  current_speed_.front_left_encoder.velocity * delta_time_;
   delta_right_ = -(current_speed_.front_right_encoder.velocity * delta_time_);
   delta_left_1_ = current_speed_.rear_left_encoder.velocity * delta_time_;
   delta_right_1_ = -(current_speed_.rear_right_encoder.velocity * delta_time_);

    // printf("delta_left_:%lf    \n",delta_left_);
  	// printf("delta_right_:%lf    \n",delta_right_);
	// printf("delta_left_1_:%lf    \n",delta_left_1_);
	// printf("delta_right_1_:%lf    \n",delta_right_1_);

	accumulation_left_ += (delta_left_/(wheel_diameter_4wd_/2.0));                  //弧度
    accumulation_right_ += (delta_right_/(wheel_diameter_4wd_/2.0));
	accumulation_right_1_ += (delta_right_1_/(wheel_diameter_4wd_/2.0));
	accumulation_left_1_ += (delta_left_1_/(wheel_diameter_4wd_/2.0));

	left_right_wheel_state_.front_left_joint.position  = accumulation_left_;
	left_right_wheel_state_.front_right_joint.position = accumulation_right_;
	left_right_wheel_state_.rear_right_joint.position = accumulation_right_1_;
	left_right_wheel_state_.rear_left_joint.position = accumulation_left_1_;

	wheel_state_join_.publish(left_right_wheel_state_);                 //  发布轮子的转动角度


	 delta_xy_ave_ = ((delta_left_+delta_left_1_)/2.0+(delta_right_+delta_right_1_)/2.0)/2.0;  //整车的位移量
	 delta_th_ =  ((delta_right_+delta_right_1_)/2.0 - (delta_left_+delta_left_1_)/2.0)/wheel_track_4wd_; //整车的角位移量


    v_linear_ = delta_xy_ave_ /delta_time_;   //速度
    v_angular_ = delta_th_ / delta_time_;    //角速度
 	// printf("v_linear_:%lf    \n",v_linear_);
  	// printf("v_angular_:%lf    \n",v_angular_);
	// printf("delta_xy_ave_:%lf    \n",delta_xy_ave_);
	// printf("delta_th_:%lf    \n",delta_th_);

    if(delta_th_ != 0)
    {
        accumulation_th_ += delta_th_;
    }
    if(delta_xy_ave_ !=0)
    {
        delta_x_ = cos(delta_th_) * delta_xy_ave_;    //X方向上的位移量
        delta_y_ = -sin(delta_th_)* delta_xy_ave_;    //Y方向上的位移量

        //累积位移量
        accumulation_x_ += (cos(accumulation_th_)*delta_x_ - sin(accumulation_th_)*delta_y_);
        accumulation_y_ += (sin(accumulation_th_)*delta_x_ + cos(accumulation_th_)*delta_y_);
		// printf("accumulation_x_:%lf   accumulation_y_:%lf  \n",accumulation_x_,accumulation_y_);
		
    }


    // printf("accumulation_th_:%lf    \n",accumulation_th_);
// 	v_linear_ = 0;
// 	v_angular_ = 0;
// 	accumulation_x_ = 0;
// 	accumulation_y_ = 0;
//    accumulation_th_ = 0;
    // transformStamped_.header.stamp    = ros::Time::now();
	transformStamped_.header.stamp    =now_;
    transformStamped_.header.frame_id = odom_frame_;
    transformStamped_.child_frame_id  = base_frame_;
    transformStamped_.transform.translation.x = accumulation_x_;
    transformStamped_.transform.translation.y = accumulation_y_;
    transformStamped_.transform.translation.z = 0.0;
    tf2::Quaternion q;
    q.setRPY(0,0,accumulation_th_);         //欧拉角转四元素
    transformStamped_.transform.rotation.x = q.x();
    transformStamped_.transform.rotation.y = q.y();
    transformStamped_.transform.rotation.z = q.z();
    transformStamped_.transform.rotation.w = q.w();

	//   transformStamped_.transform.rotation.x = 0;
    // transformStamped_.transform.rotation.y = 0;
    // transformStamped_.transform.rotation.z = 0;
    // transformStamped_.transform.rotation.w = 1;

    if(publish_odom_transform_)                     //初始化 ture
        br_.sendTransform(transformStamped_);       //发布车体角度TF坐标
    
    odom_.header.frame_id = odom_frame_;
    odom_.child_frame_id  = base_frame_;
    odom_.header.stamp    = now_;
    odom_.pose.pose.position.x = accumulation_x_;
    odom_.pose.pose.position.y = accumulation_y_;
    odom_.pose.pose.position.z = 0;
    odom_.pose.pose.orientation.x = q.getX();
    odom_.pose.pose.orientation.y = q.getY();
    odom_.pose.pose.orientation.z = q.getZ();
    odom_.pose.pose.orientation.w = q.getW();

	// odom_.pose.pose.orientation.x = 0;
    // odom_.pose.pose.orientation.y = 0;
    // odom_.pose.pose.orientation.z = 0;
    // odom_.pose.pose.orientation.w = 0;

    odom_.twist.twist.linear.x = v_linear_;
    odom_.twist.twist.linear.y = 0;
    odom_.twist.twist.angular.z = v_angular_;
    odom_.twist.covariance = { 1e-9, 0, 0, 0, 0, 0, 
                              0, 1e-3, 1e-9, 0, 0, 0, 
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0, 
                              0, 0, 0, 0, 1e6, 0, 
                              0, 0, 0, 0, 0, 1e-9 };
    odom_.pose.covariance = { 1e-9, 0, 0, 0, 0, 0, 
                              0, 1e-3, 1e-9, 0, 0, 0, 
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0, 
                              0, 0, 0, 0, 1e6, 0, 
                              0, 0, 0, 0, 0, 1e-9 };

     if (publish_odom_topic_)
    {
         odom_pub_.publish(odom_);
    }
    
    last_time_ = now_;

}


//主循环
void DriverOdomPub::loop()
{

    ros::NodeHandle n("~");
    ros::Rate loop_rate(100);

    //获取配置参数
  	n.param<std::string>("odom_frame",odom_frame_,std::string("odom"));
    n.param<std::string>("base_frame",base_frame_,std::string("base_footprint"));
    n.param<bool>("publish_odom_transform",publish_odom_transform_,false);
    n.param<bool>("publish_odom_topic",publish_odom_topic_,false);
    n.param<double>("wheel_diameter_4wd",wheel_diameter_4wd_,0.305);        //lunju
    n.param<double>("wheel_track_4wd",wheel_track_4wd_,1.08);           //0.61

    odom_pub_ = n.advertise<nav_msgs::Odometry>("/odom", 10);
    speed_pid_ = n.advertise<geometry_msgs::Twist>("/robot_cmd_vel",10);
    cmd_sub_ = n.subscribe<geometry_msgs::Twist>("/zeus_speed",100,&DriverOdomPub::cmd_vel_callback,this);      //接受上层下发的速度指令
    motor_driver_sub_ = n.subscribe<zeus_s2_msgs::Feedback>("/motor_driverFeedback",100,&DriverOdomPub::motor_feedback_callback,this);      //接受上层下发的速度指令
    wheel_state_join_=n.advertise<zeus_s2_msgs::Feedback>("/motor_join_potion",10);

    now_ = ros::Time::now();

    while(ros::ok())
    {
    
        if(cmd_vel_flag)
        {
            //发布zeus_speed话题速度  
            speed_pid_.publish(current_twist_);
            //清标志
            cmd_vel_flag = false;
        }
            
        if(rev_speed_flag)
        {
            //计算里程
            //里程发布
            handle_speed_data();
            //清标志
            rev_speed_flag = false;

        }
       
            

            ros::spinOnce();
            loop_rate.sleep();
            

    }

    


}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "Driver_s2_odom_node");
    DriverOdomPub driver_s2_odom;
    driver_s2_odom.loop();
 
    return 0;
}
